// IMU的前向传播，没有协方差矩阵的更新
#include <iostream>
#include <Eigen/Core>
#include <Sophus>

// 定义状态量
struct state
{
    Eigen::Vector3d pos = Eigen::Vector3d::Zero();
    Eigen::Vector3d vel = Eigen::Vector3d::Zero();
    Eigen::Matrix3d rot = Eigen::Matrix3d::Identity();
    Eigen::Vector3d ba = Eigen::Vector3d::Zero();
    Eigen::Vector3d bg = Eigen::Vector3d::Zero();
    Eigen::Vector3d gra = Eigen::Vector3d::Zero();
};

// 定义imu测量
struct IMU_mean
{
    Eigen::Vector3d acc = Eigen::Vector3d::Zero();
    Eigen::Vector3d gry = Eigen::Vector3d::Zero();
    double time_stamp = 0.0;
};

IMU_mean imu_last;
void predict(IMU_mean imu_s, state in_state, state out_state){
    double dt = imu_s.time_stamp - imu_last.time_stamp;

    Eigen::Vector3d gry_1 = imu_s.gry - in_state.bg;
    Eigen::Vector3d gry_2 = imu_last.gry - in_state.bg;
    Eigen::Vector3d v_gry = (gry_1 + gry_2) / 2 * dt;
    Eigen::Matrix3d rot_1 = in_state.rot * Sophus::SO3<double>::exp(v_gry).matrix();

    Eigen::Vector3d acc_1 = in_state.rot * (imu_s.acc - in_state.ba);
    Eigen::Vector3d acc_2 = rot_1 * (imu_last.acc - in_state.ba);
    Eigen::Vector3d acc_3 = (acc_1 + acc_2) / 2 - in_state.gra;

    Eigen::Vector3d vel_1 = in_state.vel + acc_3 * dt;
    Eigen::Vector3d vel_2 = (vel_1 + in_state.vel) / 2;
    Eigen::Vector3d pos_1 = in_state.pos + vel_2 * dt + 0.5 * acc_3 * dt * dt;

    imu_last = imu_s;

    out_state.pos = pos_1;
    out_state.vel = vel_1;
    out_state.rot = rot_1;
    out_state.ba = in_state.ba;
    out_state.bg = in_state.bg;
    out_state.gra = in_state.gra;
}



int main(int argc, char* argv[]){
    
    std::cout << "ok!" << std::endl;
    return 0;
}


